perm filename ARMSOL.SAI[AL,HE]1 blob
sn#290089 filedate 1977-06-28 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00005 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 ENTRY
C00006 00003 INTERNAL INTEGER PROCEDURE ARMSOL(REAL ARRAY ANGLE RPTR(FRAME,TRANS,SVAL) T
C00008 00004 ! Solve for first three joints
C00012 00005 ! Solve for last three joints
C00018 ENDMK
C⊗;
ENTRY;
BEGIN "ARMSOL"
REQUIRE "ABBREV.SAI[AL,HE]" SOURCE_FILE;
REQUIRE "RECAUX.HDR[AL,HE]" SOURCE_FILE;
EXTERNAL RCLASS V3ECT(REAL X,Y,Z);
EXTERNAL RCLASS ROTN(REAL ARRAY RMX;RPTR(V3ECT) AXIS;REAL MAGN,αL,βL);
EXTERNAL RCLASS SVAL(REAL VAL);
EXTERNAL RCLASS TRANS(RPTR(ROTN) R;RPTR(V3ECT) P);
EXTERNAL RCLASS TINFO(REAL ARRAY ANGLES);
EXTERNAL RCLASS FRAME(RPTR(TRANS) VAL;RPTR(TINFO) TINFO);
EXTERNAL RPTR(FRAME) YPARK;
EXTERNAL RPTR(FRAME) BPARK;
PRELOAD_WITH -185.0,-175.0,6.5,-175.0,-101.0,-360.0,0.0, ! Yellow low stops;
-45.0,-165.0,6.75,-395.0,-95.0,-158.0,-0.2; ! Blue low stops;
INTERNAL SAFE OWN REAL ARRAY LOSTOP[0:1,1:7];
PRELOAD_WITH 60.0,-5.0,27.5,140.0, 101.0, 360.0,3.89, ! Yellow high stops;
190.0,-50.0,33.0,205.0, 95.0, 158.0,3.80; ! Blue high stops;
INTERNAL SAFE OWN REAL ARRAY HISTOP[0:1,1:7];
PRELOAD_WITH -180.0,-90.0,14.0,-90.0,90.0,0.0, ! Yellow park position;
180.0,-90.0,14.0,-90.0,90.0,0.0; ! Blue park position;
INTERNAL SAFE OWN REAL ARRAY PARK[0:1,1:6];
PRELOAD_WITH 0.75,0.5,4.5,0.2,0.2,0.4,5.0, ! Yellow joint times;
0.75,0.75,4.5,0.2,0.2,0.4,5.0; ! Blue joint times;
INTERNAL SAFE OWN REAL ARRAY TIMFAC[0:1,1:7]; ! this is used by TCALC;
PRELOAD_WITH 29.5,8.375,16.24,6.05,36.6025,9.38,-62.5,-17.5,0.0,0.4,1.0, ! Yellow;
29.53125,50.805,20.24,-6.05,36.6025,9.281250,50.0,-53.0,0.0,0.4,1.0; ! Blue;
SAFE OWN REAL ARRAY ARM_CONST[0:1,1:11]; ! Arm constants;
! mnemonics for indexing into arm constants;
DEFINE BASEX = "1",
BASEY = "2",
S1 = "3",
S2 = "4",
S22 = "5",
S6 = "6",
MID1 = "7",
MID4 = "8",
MID6 = "9",
MIN_HAND = "10",
MIN_BOOM = "11";
DEFINE YARM = "'1",
YHAND = "'2",
BARM = "'4",
BHAND = "'10",
AHAND = "'12",
ANARM = "'5",
YELLOW = "0",
BLUE = "1";
DEFINE JT1 = "'1", JT2 = "'2", JT3 = "'4", JT4 = "'10",
JT5 = "'20", JT6 = "'40", JT7 = "'100", NZ = "'200", BAD_DATA = "'400";
DEFINE DEG = "(180.0/π)";
INTERNAL INTEGER PROCEDURE ARMSOL(REAL ARRAY ANGLE; RPTR(FRAME,TRANS,SVAL) T;
INTEGER MECH);
BEGIN ! for details of arm solution see Bruce's paper in 3rd progress report;
REAL TX,TY,TZ,SQRTXY,STH1,CTH1,STH2,CTH2,STH4,CTH4,STH6,CTH6,DIFF,ANG;
INTEGER ARM,FLAG,LOJOINT;
RPTR(ROTN) R;
RPTR(V3ECT) V;
SAFE REAL ARRAY THETA[1:6];
FLAG←0;
ARM ← IF MECH LAND (YARM LOR YHAND) THEN YELLOW ELSE BLUE;
IF MECH LAND AHAND THEN ! IT'S A HAND;
BEGIN "HANDSOL"
LOJOINT ← ARRINFO(ANGLE,1);
IF RECTYPE(T)=LOC(SVAL) THEN
BEGIN
ANGLE[LOJOINT] ← SVAL:VAL[T];
IF LOSTOP[ARM,7] ≤ SVAL:VAL[T] ≤ HISTOP[ARM,7] THEN RETURN(0);
ANGLE[LOJOINT] ← IF LOSTOP[ARM,7] > SVAL:VAL[T] THEN LOSTOP[ARM,7]
ELSE HISTOP[ARM,7];
RETURN(JT7)
END;
USERERR(0,1,"HANDSOL: BAD HAND VALUE");
RETURN(BAD_DATA)
END "HANDSOL";
IF (ARM=YELLOW ∧ T=YPARK) ∨ (ARM=BLUE ∧ T=BPARK) THEN
BEGIN "park position is always the same"
ARRBLT(ANGLE[1],PARK[ARM,1],6);
RETURN(0)
END;
IF RECTYPE(T)=LOC(FRAME) THEN T←FRAME:VAL[T]
ELSE IF RECTYPE(T)≠LOC(TRANS) THEN
BEGIN
USERERR(0,1,"ARMSOL: BAD ARM VALUE");
RETURN(BAD_DATA)
END;
! Solve for first three joints;
R←TRANS:R[T];
V←TRANS:P[T];
! Solve for end of boom;
TX ← V3ECT:X[V] - ROTN:RMX[R][3,1]*ARM_CONST[ARM,S6] - ARM_CONST[ARM,BASEX];
TY ← V3ECT:Y[V] - ROTN:RMX[R][3,2]*ARM_CONST[ARM,S6] - ARM_CONST[ARM,BASEY];
TZ ← V3ECT:Z[V]; ! Test that we're not trying to reach through the table;
IF TZ<ARM_CONST[ARM,MIN_HAND] THEN BEGIN FLAG←NZ; TZ←ARM_CONST[ARM,MIN_HAND] END;
TZ ← TZ - ROTN:RMX[R][3,3]*ARM_CONST[ARM,S6];
IF TZ<ARM_CONST[ARM,MIN_BOOM] THEN BEGIN FLAG←NZ; TZ←ARM_CONST[ARM,MIN_BOOM] END;
IF (SQRTXY←TX↑2+TY↑2-ARM_CONST[ARM,S22])<0 THEN
BEGIN FLAG ← FLAG LOR JT1; SQRTXY←0 END ELSE SQRTXY←-SQRT(SQRTXY);
! Solve joint 1;
THETA[1] ← IF ABS(TY+ARM_CONST[ARM,S2])>0.001 THEN
IF TY+ARM_CONST[ARM,S2]>0 THEN 2*ATAN2(-TX+SQRTXY,TY+ARM_CONST[ARM,S2])*DEG
ELSE 2*ATAN2(TX-SQRTXY,-TY-ARM_CONST[ARM,S2])*DEG
ELSE IF TX<0 THEN 2*ATAN2(-TY,-TX)*DEG ELSE 180.0;
IF THETA[1]>HISTOP[ARM,1] THEN THETA[1]←THETA[1]-360;
IF THETA[1]<LOSTOP[ARM,1] THEN THETA[1]←THETA[1]+360;
IF THETA[1]>HISTOP[ARM,1] THEN ! Outside joint limits;
BEGIN
FLAG ← FLAG LOR JT1;
THETA[1] ← IF ANGLE[1] > ARM_CONST[ARM,MID1] THEN HISTOP[ARM,1]
ELSE LOSTOP[ARM,1]
END;
STH1 ← SIND(THETA[1]);
CTH1 ← COSD(THETA[1]);
! Solve for joint 2;
THETA[2] ← ATAN2(SQRTXY,TZ-ARM_CONST[ARM,S1])*DEG;
IF THETA[2] < LOSTOP[ARM,2] THEN
BEGIN FLAG←FLAG LOR JT2; THETA[2] ← LOSTOP[ARM,2] END
ELSE IF THETA[2] > HISTOP[ARM,2] THEN
BEGIN FLAG←FLAG LOR JT2; THETA[2] ← HISTOP[ARM,2] END; ! Check joint limits;
STH2 ← SIND(THETA[2]);
CTH2 ← COSD(THETA[2]);
! Solve for joint 3;
THETA[3] ← (TX*CTH1 +TY*STH1)/STH2;
IF THETA[3] < LOSTOP[ARM,3] THEN
BEGIN FLAG←FLAG LOR JT3; THETA[3] ← LOSTOP[ARM,3] END
ELSE IF THETA[3] > HISTOP[ARM,3] THEN
BEGIN FLAG←FLAG LOR JT3; THETA[3] ← HISTOP[ARM,3] END; ! Check joint limits;
! Solve for last three joints;
STH4 ← CTH2*(CTH1*ROTN:RMX[R][3,1]+STH1*ROTN:RMX[R][3,2])-STH2*ROTN:RMX[R][3,3];
CTH4 ← STH1*ROTN:RMX[R][3,1] - CTH1*ROTN:RMX[R][3,2];
! Solve for joint 5;
THETA[5] ← ABS(ATAN2(SQRT(STH4↑2+CTH4↑2),
STH2*(CTH1*ROTN:RMX[R][3,1]+STH1*ROTN:RMX[R][3,2])+CTH2*ROTN:RMX[R][3,3])*DEG);
IF ANGLE[5]<0 THEN THETA[5]←-THETA[5]; ! Use same side as old position;
IF THETA[5] < LOSTOP[ARM,5] THEN
BEGIN FLAG←FLAG LOR JT5; THETA[5] ← LOSTOP[ARM,5] END
ELSE IF THETA[5] > HISTOP[ARM,5] THEN
BEGIN FLAG←FLAG LOR JT5; THETA[5] ← HISTOP[ARM,5] END; ! Check joint limits;
IF ABS(THETA[5])<0.1 THEN
BEGIN "degenerate case"
THETA[4]←ANGLE[4];
THETA[6]←(ANG←ATAN2(ROTN:RMX[R][1,3],ROTN:RMX[R][2,3])*DEG) - THETA[4];
IF THETA[6]>HISTOP[ARM,6] THEN THETA[6]←THETA[6]-360;
IF THETA[6]<LOSTOP[ARM,6] THEN THETA[6]←THETA[6]+360;
IF THETA[6]>HISTOP[ARM,6] THEN ! Outside joint limits;
BEGIN ! Have to move joint 4 too;
DIFF←THETA[6];
THETA[6]←IF (THETA[6]-180)>ARM_CONST[ARM,MID6] THEN HISTOP[ARM,6]
ELSE LOSTOP[ARM,6];
DIFF←DIFF-THETA[6];
THETA[4]←THETA[4]+DIFF;
IF THETA[4]>HISTOP[ARM,4] THEN THETA[4]←THETA[4]-360;
IF THETA[4]<LOSTOP[ARM,4] THEN THETA[4]←THETA[4]+360;
IF THETA[4]>HISTOP[ARM,4] THEN ! Outside joint limits;
BEGIN ! Try flipping joint 4 by 180;
THETA[4]←IF ANGLE[4]>ARM_CONST[ARM,MID4] THEN
ANGLE[4]-180.0 ELSE ANGLE[4]-180;
IF THETA[4] < LOSTOP[ARM,4] THEN THETA[4] ← LOSTOP[ARM,4]
ELSE IF THETA[4] > HISTOP[ARM,4] THEN THETA[5] ← HISTOP[ARM,5];
! Check joint limits;
THETA[6] ← ANG - THETA[4];
IF THETA[6]>HISTOP[ARM,6] THEN THETA[6]←THETA[6]-360
ELSE IF THETA[6]<LOSTOP[ARM,6] THEN THETA[6]←THETA[6]+360;
END
END
END "degenerate case"
ELSE
BEGIN "normal case"
! Solve for joint 4;
THETA[4] ← IF THETA[5]>0 THEN ATAN2(STH4,CTH4)*DEG ELSE ATAN2(-STH4,-CTH4)*DEG;
IF (DIFF←ANGLE[4]-THETA[4])>180 THEN
BEGIN THETA[4]←THETA[4]-360; DIFF←DIFF-360 END
ELSE IF DIFF<-180 THEN BEGIN THETA[4]←THETA[4]+360; DIFF←DIFF+360 END;
IF DIFF>90 THEN BEGIN THETA[4]←THETA[4]-180; THETA[5]←-THETA[5] END
ELSE IF DIFF<-90 THEN BEGIN THETA[4]←THETA[4]+180; THETA[5]←-THETA[5] END;
IF THETA[4]<LOSTOP[ARM,4] THEN
BEGIN THETA[4]←THETA[4]+180; THETA[5]←-THETA[5] END
ELSE IF THETA[4]>HISTOP[ARM,4] THEN
BEGIN THETA[4]←THETA[4]-180; THETA[5]←-THETA[5] END;
! Solve for joint 6;
STH6←STH2*(CTH1*ROTN:RMX[R][2,1]+STH1*ROTN:RMX[R][2,2])+CTH2*ROTN:RMX[R][2,3];
CTH6←-STH2*(CTH1*ROTN:RMX[R][1,1]+STH1*ROTN:RMX[R][1,2])-CTH2*ROTN:RMX[R][1,3];
THETA[6] ← IF THETA[5]>0 THEN ATAN2(STH6,CTH6)*DEG ELSE ATAN2(-STH6,-CTH6)*DEG;
IF THETA[6]<LOSTOP[ARM,6] THEN THETA[6]←THETA[6]+360;
IF THETA[6]>HISTOP[ARM,6] THEN THETA[6]←THETA[6]-360;
IF THETA[6]<LOSTOP[ARM,6] THEN ! Try alternate solution;
IF LOSTOP[ARM,4]≤(ANG←IF THETA[4]>ARM_CONST[ARM,MID4] THEN
THETA[4]-180.0 ELSE THETA[4]+180.0)≤HISTOP[ARM,4] THEN
BEGIN "flip wrist"
THETA[6] ← THETA[6] + 180.0;
THETA[5] ← -THETA[5];
THETA[4] ← ANG
END
ELSE
BEGIN "no solution"
FLAG ← FLAG LOR JT6;
THETA[6]←IF (THETA[6]+180.0)>ARM_CONST[ARM,MID6] THEN HISTOP[ARM,6]
ELSE LOSTOP[ARM,6];
END
END "normal case";
ARRBLT(ANGLE[1],THETA[1],6);
RETURN(FLAG)
END;
END "ARMSOL"